/*
 * MotorControl.c
 *
 *  Created on: Nov 5, 2012
 *      Author: banfield
 */

/*****************************************************
 * Include Statements
 ****************************************************/
#include "hw_types.h"
#include "gpio.h"
#include <stdlib.h>
#include <stdio.h>
#include "inc/lm3s2965.h"
#include "MotorControl.h"
#include "interrupt.h"
#include "sysctl.h"
#include "hw_memmap.h"
#include "hw_types.h"
#include "hw_ints.h"

/*****************************************************
 * Define Statements
 ****************************************************/
#define MOTOR_PORT GPIO_PORTG_DATA_R
#define STEP_PORT GPIO_PORTD_DATA_R

//#define X_MOTOR 0b1
//#define Y_MOTOR 0b10
//#define PHI_MOTOR 0b100
#define DIR_PIN 0b1000
#define STEP_PIN 0b10
#define	STEP_TRANS_DIST 0.007854	//translational distance from one 1.8 degree step (based on spur pitch diameter)
#define STEP_ROT_DIST	0.0001		//rotational increment per step (radians)
#define	SLOP_MARGIN		1.2			//slop percentage (120%)
#define	STEP_DELAY 10000

/*****************************************************
 * Module Level Variables
 ****************************************************/
static float currentX;
static float currentY;
static float currentPhi;
static char xHomeFlag;
static char yHomeFlag;
static char homingX;
static char homingY;

//driver board is hard-wired to full step mode

void InitMotors(void){
	unsigned long dummy;

    // Enable the GPIO ports that is used for motor control.
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);

    // Do a dummy read to insert a few cycles after enabling the peripheral.
    dummy = SYSCTL_RCGC2_R;

    //Enable the GPIO port that used for homing interrupts
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);

	//Enable motor interrupts
	IntEnable(INT_GPIOC);
	GPIOIntTypeSet(GPIO_PORTC_BASE, 0b00110000,GPIO_RISING_EDGE);
	GPIOPinIntEnable(GPIO_PORTC_BASE,0b00110000);

    // Enable the GPIO pins for motor control.  Set the direction as output, and
    // enable the GPIO pin for digital function.
    GPIO_PORTG_AFSEL_R &= ~0b1111;
    GPIO_PORTG_DIR_R |= 0b1111;
    GPIO_PORTG_DEN_R |= 0b1111;

    GPIO_PORTD_AFSEL_R &= ~0b10;
    GPIO_PORTD_DIR_R |= 0b10;
    GPIO_PORTD_DEN_R |= 0b10;

	currentX = 0;
	currentY = 0;
	currentPhi = 0;

	homingX = 0;
	homingY = 0;

	xHomeFlag = 0;
	xHomeFlag = 0;
}

void SetDirection(int *steps){
	if((steps != NULL) && (*steps > 0)){
		MOTOR_PORT |= DIR_PIN;
	}else{
		*steps = -*steps;
		MOTOR_PORT &= ~DIR_PIN;
	}
}

void SetMotor(char motor){
	// write motor selection to stellaris
	MOTOR_PORT &= 0b11111000;
	MOTOR_PORT |= motor;	//write the correct binary value to the motor port
}

void Step(void){
	int i;

	STEP_PORT |= STEP_PIN;
	//this loop was bam bams idea, I said ASM no-ops (ichiban)
	for(i = 0 ; i < STEP_DELAY ; i++);

	STEP_PORT &= ~STEP_PIN;

	//this loop was bam bams idea, I said ASM no-ops (ichiban)
	for(i = 0 ; i < STEP_DELAY ; i++);
}


float SingleStepMotor(char motor, int direction){
	SetDirection(&direction);
	SetMotor(motor);
	Step();

	if(motor == X_MOTOR){
		currentX += STEP_TRANS_DIST;
		return currentX;
	}else if(motor == Y_MOTOR){
		currentY += STEP_TRANS_DIST;
		return currentY;
	}else{
		currentPhi += STEP_ROT_DIST;
		return currentPhi;
	}
}


static void MoveMotor(char motor, float endPos, float *currentPos){
	int steps, i;

	if(motor == PHI_MOTOR){
		steps = (endPos - *currentPos)/STEP_ROT_DIST;
	}else{
		steps = (endPos - *currentPos)/STEP_TRANS_DIST;
	}

	SetMotor(motor);
	SetDirection(&steps);
	for(i = 0 ; i < steps ; i++){
		Step();
	}

	*currentPos = endPos;
}

void MoveToCoord(float x, float y, float phi){
	//TODO: need to add limit checking on x, y, and phi
	printf("MotorControl: MoveToCoord x = %f, y = %f, phi = %f\n", x, y, phi);
	MoveMotor(X_MOTOR, x, &currentX);
	MoveMotor(Y_MOTOR, y, &currentY);
	MoveMotor(PHI_MOTOR, phi, &currentPhi);
}

static void ResetMotor(char motor, float *position, char *homeFlag){
	SetMotor(motor);
	SetDirection(NULL);

	while(!(*homeFlag)){
		Step();
	}

	*position = 0;
}

void ReturnHome(void){
	homingX = 1;
	homingY = 1;

	ResetMotor(X_MOTOR, &currentX, &xHomeFlag);
	ResetMotor(Y_MOTOR, &currentY, &yHomeFlag);
	//ResetMotor(PHI_MOTOR, &currentPhi, PHI_HOME);

	homingX = 0;
	homingY = 0;
}

float GetCurrentX(void){
	return currentX;
}

float GetCurrentY(void){
	return currentX;
}

float GetCurrentPhi(void){
	return currentPhi;
}

void GPIO_PORTC_Int_Handler(void){
        unsigned long ulStatus;
        ulStatus = GPIOPinIntStatus(GPIO_PORTC_BASE, true);
        GPIOPinIntClear(GPIO_PORTC_BASE, ulStatus);
        switch(ulStatus&&0b00110000){
                case 0b00010000:
                        //Stop Motor x

                        if(homingX==1)//We were trying to home X
                        {
                                //Post an event
                                xHomeFlag = 1;
                        }
                        else{//We got here by accident - stay put
                                while(1);
                        }
                break;
                case 0b00100000:
                        //Stop Motor y
                        if(homingY==1)//We were trying to home Y
                        {
                                //Post an event
                                yHomeFlag = 1;
                        }
                        else{//We got here by accident - stay put
                                while(1);
                        }
                break;
        }
}
